import math
import numpy as np
import numpy.random
# import xlwt


class Geolation:
    def __init__(self, time: int, latitude: float, longitude: float):
        self.time = time
        self.latitude = latitude                          # x轴, 纬度
        self.longitude = longitude


def print_geo(geolation: Geolation):
    print('['+str(geolation.time) + ',', str(geolation.latitude) + ',', str(geolation.longitude) + ']')


def print_geolist(tra: list[Geolation]):
    for geo in tra:
        print_geo(geo)


# 定义速度函数， 其返回值为两个坐标点间的距离（不是速度）
class Speed:
    def get_speed(self) -> float:
        return 0.0


class KeepSpeed(Speed):
    speed = 5.0

    def __init__(self, speed):
        self.speed = speed

    def get_speed(self) -> float:
        return self.speed


class MatchSpeed(Speed):
    avg = 6.1412
    variance = 0.7164

    def __init__(self, avg: float, variance: float):
        self.avg = avg
        self.variance = variance

    def get_speed(self) -> float:
        return np.random.normal(self.avg, self.variance)


class Trajectory:
    @staticmethod
    def get_trajectory(vector: Speed = MatchSpeed(6.1412, 0.3164)) -> list[Geolation]:
        dt = 2000                                 # 每个坐标点时间差
        std_r = 0.563                             # 标准化半径
        r = 40.3216933181318                      # 重整半径
        std_numerator = 0.000748225519274115      # 标准化的分子
        l = 72.7157904780655                      # 直道长
        diffly = 0.012                            # 直道y标准差
        diffr = 0.013                              # 弯道长度标准差
        dx = 36.12432223
        dy = 120.4846737

        extent = (2*r + l)/2                      # 重整化参数
        center = 1 - std_r                        # 圆心坐标

        now = Geolation(0, np.random.normal(0, 0.01), np.random.normal(-std_r, diffly))
        ret = [now]
        length = 0.0

        # 标准化下进行生成
        while length < 2000:
            ds = vector.get_speed()
            print(ds)
            new_point = Geolation(0, 0, 0)
            if -center <= now.latitude <= center and now.longitude <= 0.0:        # 西侧直道
                theory_point = Geolation(now.time, now.latitude, -std_r)
                new_point = Geolation(theory_point.time + dt, theory_point.latitude - ds / extent, np.random.normal(-std_r, diffly))
            elif now.latitude < -center:                                          # 南侧弯道
                zeta = math.atan(now.longitude/(now.latitude + center)) + math.pi
                target_zeta = zeta - ds / r
                target_rou = np.random.normal(std_r, diffr)
                new_point = Geolation(now.time + dt, target_rou*math.cos(target_zeta) - center, target_rou*math.sin(target_zeta))
            elif -center <= now.latitude <= center and now.longitude > 0.0:
                theory_point = Geolation(now.time, now.latitude, std_r)
                new_point = Geolation(theory_point.time + dt, theory_point.latitude + ds / extent, np.random.normal(std_r, diffly))
            elif now.latitude > center:
                zeta = math.atan(now.longitude/(now.latitude - center))
                target_zeta = zeta - ds / r
                target_rou = np.random.normal(std_r, diffr)
                new_point = Geolation(now.time + dt, target_rou*math.cos(target_zeta) + center, target_rou*math.sin(target_zeta))

            ret.append(new_point)
            now = new_point
            length += ds

        # 反标准化
        for point in ret:
            point.latitude = point.latitude * std_numerator + dx
            point.longitude = point.longitude * std_numerator + dy

        return ret

